// -------------------------------------------------------------------------
// -----                EiccFdtIdealRecoTask source file             -----
// -----                  Created 23/01/10  by Aiqiang Guo           -----
// -------------------------------------------------------------------------
// Note for v1: 
// Implement the track fitting for the Fdt tracker
// By Aiqiang Guo -- Jan/10/2023
// -------------------------------------------------------------------------
// libc includes
#include <iostream>

// Root includes
#include "TROOT.h"
#include "TClonesArray.h"
#include "TParticlePDG.h"
#include "TRandom.h"
#include "TGeoManager.h"
#include "TGeoMatrix.h"
#include "TFile.h"

// framework includes
#include "FairRootManager.h"
#include "EiccFdtIdealRecoTask.h"
#include "FairRun.h"
#include "FairRuntimeDb.h"
//#include "../EiccData/EiccMCTrack.h"
#include "EiccMCTrack.h"
#include "FairHit.h"
// EiccFdt includes
//#include "EiccFdtHit.h"

#include "EiccFdtPoint.h"

#include "EiccConstField.h"  //change genfit to access FairField 01062023 Yutie
#include "EiccFieldMap.h"
#include "EiccQuadMap.h"
#include "EiccMultiField.h"

#include <iostream>
#include <execinfo.h>
#include <signal.h>
#include <stdlib.h>

//Genfit2 
#include <AbsFinitePlane.h>
#include <AbsFitterInfo.h>
#include <AbsMeasurement.h>
#include <AbsTrackRep.h>
#include <ConstField.h>
#include <FieldMap.h> //Yutie Liang 2022.02.15
#include <DetPlane.h>
#include <Exception.h>
#include <FieldManager.h>
#include <KalmanFittedStateOnPlane.h>
#include <AbsKalmanFitter.h>
#include <KalmanFitter.h>
#include <KalmanFitterRefTrack.h>
#include <KalmanFitterInfo.h>
#include <KalmanFitStatus.h>
#include <DAF.h>
#include <GFGbl.h>
#include <MeasuredStateOnPlane.h>
#include <MeasurementOnPlane.h>
#include <FullMeasurement.h>
#include <PlanarMeasurement.h>
#include <ProlateSpacepointMeasurement.h>
#include <RectangularFinitePlane.h>
#include <ReferenceStateOnPlane.h>
#include <SharedPlanePtr.h>
#include <SpacepointMeasurement.h>
#include <StateOnPlane.h>
#include <Tools.h>
#include <TrackCand.h>
#include <TrackCandHit.h>
#include <Track.h>
#include <TrackPoint.h>
#include <WireMeasurement.h>
#include <WirePointMeasurement.h>

#include <MaterialEffects.h>
#include <RKTools.h>
#include <RKTrackRep.h>
#include <StepLimits.h>
#include <TGeoMaterialInterface.h>

#include <EventDisplay.h>

#include <HelixTrackModel.h>
#include <MeasurementCreator.h>

#include <GFRaveVertexFactory.h>
#include <rave/Version.h>    //test rave
#include <rave/Track.h>
#include <rave/Exception.h>
#include <rave/VertexFactory.h>
#include <rave/ConstantMagneticField.h>

#ifdef VALGRIND
#include <valgrind/callgrind.h>
#else
#define CALLGRIND_START_INSTRUMENTATION
#define CALLGRIND_STOP_INSTRUMENTATION
#define CALLGRIND_DUMP_STATS
#endif


// -----   Default constructor   -------------------------------------------
EiccFdtIdealRecoTask::EiccFdtIdealRecoTask() :
    FairTask("Ideal reconstruction task for EiccFdt"),
    fPersistance(kTRUE),
    fBranchName("EiccFdtPoint"),
    fPointArray(NULL),
    fMctruthArray(NULL),
    fHitOutputArray(NULL),
    fSigmaX(0.),
    fSigmaY(0.),
    fSigmaZ(0.),
    fDebug(1),
    fCurrentEiccFdtPoint(NULL),
    fCurrentTransMat(NULL),
    fBtype("QUAD"),
    fOutname("/public/hep2/guoaq/GenFit_out_put/Eicc_Fdt_QUAD_performance.csv"),
    fOutEffname("/public/hep2/guoaq/GenFit_out_put/Eicc_Edt_QUAD_performance_Eff.csv"),
    fHitCovMatrix(3,3)
{
}
// fout.open("/public/hep2/guoaq/GenFit_out_put/Eicc_Fdt_QUAD_performance.csv",std::ios::app);
// -------------------------------------------------------------------------

// -----   Constructor   ---------------------------------------------------
EiccFdtIdealRecoTask::EiccFdtIdealRecoTask(Double_t sx, Double_t sy, Double_t sz, bool debug, TString foutname,  TString fouteffname, TString fbtype) :
    FairTask("Ideal reconstruction task for EiccFdt"),
    fPersistance(kTRUE),
    fBranchName("EiccFdtPoint"),
    fPointArray(NULL),
    fMctruthArray(NULL),
    fHitOutputArray(NULL),
    fSigmaX(sx),
    fSigmaY(sy),
    fSigmaZ(sz),
    fDebug(debug),
    fCurrentEiccFdtPoint(NULL),
    fCurrentTransMat(NULL),
    fOutname(foutname),
    fOutEffname(fouteffname),
    fBtype(fbtype),
    fHitCovMatrix(3,3)
{
}
// -------------------------------------------------------------------------


// -----   Destructor   ----------------------------------------------------
EiccFdtIdealRecoTask::~EiccFdtIdealRecoTask()
{

}


void EiccFdtIdealRecoTask::FinishTask()
{
    delete fitTrack;
    delete secondTrack;
    delete fitter;
    fout.close();
    fouteff.close();
    //t_out->Write();
    //f_data_out->Close();

    CALLGRIND_STOP_INSTRUMENTATION;
    CALLGRIND_DUMP_STATS;

    std::cout<<"~EiccFdtIdealRecoTask Destructor"<<std::endl;
    //display->setOptions("ABDEFHMPT"); // G show geometry
    //if(matFX) display->setOptions("ABDEFGHMPT"); // G show geometry
    if(DisplayEvent)display->setOptions("ABDEFHMPTXG"); // G show geometry
    if(matFX&&DisplayEvent) display->setOptions("ABDEFGHMPTXG"); // G show geometry
    if(DisplayEvent)gGeoManager->SetVisLevel(4);
    if(DisplayEvent)display->open();


}

// -----   Public method Init   --------------------------------------------
InitStatus EiccFdtIdealRecoTask::Init()
{
    // Get RootManager
    FairRootManager* ioman = FairRootManager::Instance();
    if ( ! ioman ) {
	std::cout << "-E- EiccFdtIdealRecoTask::Init: "
	    << "RootManager not instantiated!" << std::endl;
	return kFATAL;  }

    // Get input array
    fPointArray = (TClonesArray*) ioman->GetObject(fBranchName);
    if ( ! fPointArray ) {
	std::cout << "-W- EiccFdtIdealRecoTask::Init: "<< "No "<<fBranchName
	    <<" array!" << std::endl;
	return kERROR;  }

    // Get MCTruth collection
    fMctruthArray=(TClonesArray*) ioman->GetObject("MCTrack");
    if(fMctruthArray==0)  {
	std::cout << "-W- EiccFdtIdealRecoTask::Init: No McTruth array!" << std::endl;
	return kERROR;  }


    // Create and register output array
    fHitOutputArray = new TClonesArray("EiccFdtHit");
    ioman->RegisterAny("EiccFdtHit", fHitOutputArray, fPersistance);

    std::cout << "-I- gGeoManager = "<<gGeoManager << std::endl;

    std::cout<<"start initialization of display......"<<std::endl;

    wireDir.SetXYZ(0,0,1);

    for (unsigned int i = 0; i<nMeasurements; ++i) {
	measurementTypes.push_back(genfit::eMeasurementType(i%8));
    }

    //  signal(SIGSEGV, handler);   // install our handler

    // init fitter
    switch (fitterId) {
	case genfit::SimpleKalman:
	    fitter = new genfit::KalmanFitter(nIter, dPVal);
	    fitter->setMultipleMeasurementHandling(mmHandling);
	    break;

	case genfit::RefKalman:
	    fitter = new genfit::KalmanFitterRefTrack(nIter, dPVal);
	    fitter->setMultipleMeasurementHandling(mmHandling);
	    break;

	case genfit::DafSimple:
	    fitter = new genfit::DAF(false);
	    break;
	case genfit::DafRef:
	    fitter = new genfit::DAF();
	    break;
    }
    fitter->setMaxIterations(nIter);


    // init MeasurementCreator
    measurementCreator.setResolution(resolution);
    measurementCreator.setResolutionWire(resolutionWire);
    measurementCreator.setOutlierProb(outlierProb);
    measurementCreator.setOutlierRange(outlierRange);
    measurementCreator.setThetaDetPlane(thetaDetPlane);
    measurementCreator.setPhiDetPlane(phiDetPlane);
    measurementCreator.setWireDir(wireDir);
    measurementCreator.setMinDrift(minDrift);
    measurementCreator.setMaxDrift(maxDrift);
    measurementCreator.setIdealLRResolution(idealLRResolution);
    measurementCreator.setUseSkew(useSkew);
    measurementCreator.setSkewAngle(skewAngle);
    measurementCreator.setNSuperLayer(nSuperLayer);
    measurementCreator.setDebug(fDebug);

    /*
    // init geometry and mag. field
    new TGeoManager("Geometry", "Geane geometry");
    //TGeoManager::Import("genfitGeom.root");
    TGeoManager::Import("geofile_full.root");
     */  
    //genfit::FieldManager::getInstance()->init(new genfit::ConstField(0.,0.,15));
    /*
       EiccConstField* fMagField = new EiccConstField();
       fMagField->SetField(0., 0., 15.);
       fMagField->SetFieldRegion(-200, 200, -200, 200, -200, 200);
     */

    EiccMultiField *fMagField = new EiccMultiField(fBtype);
    fMagField->Init();
    genfit::FieldManager::getInstance()->init(fMagField);

    // genfit::FieldMap *fieldmap = new genfit::FieldMap();
    // fieldmap->ReadAsciiFile("map.dat");

    genfit::FieldManager::getInstance()->useCache(true, 8);
    genfit::MaterialEffects::getInstance()->init(new genfit::TGeoMaterialInterface());

    // init fitter
    genfit::AbsKalmanFitter* fitter = new genfit::KalmanFitterRefTrack();

    std::cout<<"GFRaveVertexFactory .... "<<std::endl;
    // test vertex factory

    // genfit::GFRaveVertexFactory vertexFactory(2);
    // vertexFactory.setMethod("kalman-smoothing:1");//What does 1 mean? - Guoaq-2021/Apr/26


    // init event display
#ifndef VALGRIND
    if(DisplayEvent) display = genfit::EventDisplay::getInstance();
    if(DisplayEvent) display->reset();
    // std::cout<<"====display init...."<<std::endl;
#endif

    /*
    // Create Root file for the information you are interested
    fout_name = Form("/public/hep2/guoaq/GenFit_out_put/output/EicC_ffd_performance.root");
    f_data_out = new TFile(fout_name,"recreate");
    t_out = new TTree("t_out","t_out");

    // double px_truth, py_truth, pz_truth, pt_truth, p_truth, theta_truth, eta_truth;
    // double px_rec, py_rec, pz_rec, pt_rec, p_rec, theta_rec, eta_rec;
    // double mom_res, mom_res_old, dca_rphi, dca_z;
    // double x_truth[100], y_truth[100], z_truth[100], r_truth[100], phi_truth[100], det_id[100], Evt_id[100];
    // double x_rec[100], y_rec[100], z_rec[100], phi_rec[100];
    // double chi2;
    // double abs_dca_rphi, dca_x, dca_y;
    // int NHit;

    t_out->Branch("px_truth", &px_truth, "px_truth/D");
    t_out->Branch("py_truth", &py_truth, "py_truth/D");
    t_out->Branch("pz_truth", &pz_truth, "pz_truth/D");
    t_out->Branch("pt_truth", &pt_truth, "pt_truth/D");
    t_out->Branch("p_truth", &p_truth, "p_truth/D");
    t_out->Branch("theta_truth", &theta_truth, "theta_truth/D");
    t_out->Branch("eta_truth", &eta_truth, "eta_truth/D");
    t_out->Branch("px_rec", &px_rec, "px_rec/D");
    t_out->Branch("py_rec", &py_rec, "py_rec/D");
    t_out->Branch("pz_rec", &pz_rec, "pz_rec/D");
    t_out->Branch("pt_rec", &pt_rec, "pt_rec/D");
    t_out->Branch("p_rec", &p_rec, "p_rec/D");
    t_out->Branch("theta_rec", &theta_rec, "theta_rec/D");
    t_out->Branch("eta_rec", &eta_rec, "eta_rec/D");
    t_out->Branch("mom_res_old", &mom_res_old, "mom_res_old/D");
    t_out->Branch("mom_res", &mom_res, "mom_res/D");
    t_out->Branch("dca_rphi", &dca_rphi, "dca_rphi/D");
    t_out->Branch("abs_dca_rphi", &abs_dca_rphi, "abs_dca_rphi/D");
    t_out->Branch("dca_x", &dca_x, "dca_x/D");
    t_out->Branch("dca_y", &dca_y, "dca_y/D");
    t_out->Branch("dca_z", &dca_z, "dca_z/D");
    t_out->Branch("chi2", &chi2, "chi2/D");
    t_out->Branch("NHit", &NHit, "NHit/I");
     */

    // fout.open("/public/hep2/guoaq/GenFit_out_put/Eicc_Fdt_QUAD_performance.csv",std::ios::app);
    fout.open(fOutname,std::ios::app);
    fouteff.open(fOutEffname,std::ios::app);

    fEventNo = -1;

    return kSUCCESS;

}
// -------------------------------------------------------------------------
void EiccFdtIdealRecoTask::SetParContainers()
{
    // Get Base Container
    //  FairRun* ana = FairRun::Instance();
    //  FairRuntimeDb* rtdb=ana->GetRuntimeDb();
    //	EiccSdsIdealRecoTask::SetParContainers();
}


// -----   Public method Exec   --------------------------------------------
void EiccFdtIdealRecoTask::Exec(Option_t*)
{

    fEventNo++;
    //std::cout<<"Event ID is "<<fEventNo<<std::endl;
    if(fEventNo<1000&&fEventNo%100==0)std::cout<<"Event ID is "<<fEventNo<<std::endl;
    else if(fEventNo%1000==0)std::cout<<"Event ID is "<<fEventNo<<std::endl;

    if ( ! fHitOutputArray ) Fatal("Exec", "No fHitOutputArray");
    fHitOutputArray->Delete();
    fHitOutputArray->Clear();

    if(fDebug)std::cout<<"start process the track fitting....."<<std::endl;
    //std::cout << "This is Rave Version " << rave::Version() << std::endl;

    //rave::ConstantMagneticField mfield(0.,0., 15);
    //rave::VertexFactory factory  (mfield, rave::VacuumPropagator(), "kalman", 0 );
    //Test genfit

    double maxWeight(0);
    unsigned int nTotalIterConverged(0);
    unsigned int nTotalIterNotConverged(0);
    unsigned int nTotalIterSecondConverged(0);
    unsigned int nTotalIterSecondNotConverged(0);
    unsigned int nConvergedFits(0);
    unsigned int nUNConvergedFits(0);
    unsigned int nConvergedFitsSecond(0);
    unsigned int nUNConvergedFitsSecond(0);

    int fs = 0;

    // clean up
    //delete fitTrack;
    fitTrack = nullptr;
    //delete secondTrack;
    secondTrack = nullptr;

    if(fDebug)std::cout<<"before initialization the variable"<<std::endl;

    for(int k=0;k<nMaxHit;k++)
    {
	X[k]=-100;
	Y[k]=-100;
	Z[k]=-100;
	PX[k]=-100;
	PY[k]=-100;
	PZ[k]=-100;
	DetID[k]=-100;
    }

    // double px_truth, py_truth, pz_truth, pt_truth, p_truth, theta_truth, eta_truth;
    // double px_rec, py_rec, pz_rec, pt_rec, p_rec, theta_rec, eta_rec;
    // double mom_res, dca_rphi, dca_z;
    //int NHit;

    if(fDebug)std::cout<<"before draw the geometry"<<std::endl;
    gGeoManager->GetTopVolume()->Draw();

    if(fDebug)std::cout<<"fetch the truth info"<<std::endl;

    // fMctruthArray=(TClonesArray*) ioman->GetObject("MCTrack");

    double TruPx, TruPy, TruPz, TruP, TruPt, TruEta;
    Int_t nEiccEdtTracks=fMctruthArray->GetEntriesFast();
    for(Int_t iEdtTrack=0;iEdtTrack<nEiccEdtTracks;++iEdtTrack)
    {
	fCurrentEiccMCTrack=(EiccMCTrack*)fMctruthArray->At(iEdtTrack);
	if(fCurrentEiccMCTrack->GetMotherId()==-1)
	{
	    TruPx = fCurrentEiccMCTrack->GetPx();
	    TruPy = fCurrentEiccMCTrack->GetPy();
	    TruPz = fCurrentEiccMCTrack->GetPz();
	    TruP = fCurrentEiccMCTrack->GetP();
	    TruPt = fCurrentEiccMCTrack->GetPt();
	    break;
	}

    }//end for EiccEdtiEdtPoint


    if(fDebug)std::cout<<"fetch the hit info"<<std::endl;

    Int_t size = 0;
    Int_t nEiccFdtPoints=fPointArray->GetEntriesFast();
    if(fDebug)std::cout<<"Hit number is "<<nEiccFdtPoints<<std::endl;
    for(Int_t iFdtPoint=0;iFdtPoint<nEiccFdtPoints;++iFdtPoint)
    {
	fCurrentEiccFdtPoint=(EiccFdtPoint*)fPointArray->At(iFdtPoint);
	if(fDebug)std::cout<<__LINE__<<std::endl;
	Int_t trackid=fCurrentEiccFdtPoint->GetTrackID();
	if(fDebug)std::cout<<__LINE__<<std::endl;
	InitTransMat();
	if(fDebug)std::cout<<__LINE__<<std::endl;

	if(trackid!=0)continue;
	if(fDebug)std::cout<<__LINE__<<std::endl;

	TVector3 o(0.,0.,0.),u(1.,0.,0.),v(0.,1.,0.);
	CalcDetPlane(o,u,v);
	TVector3 pos_hit, mom_hit;
	fCurrentEiccFdtPoint->Position(pos_hit);
	fCurrentEiccFdtPoint->Momentum(mom_hit);

	TGeoNode* node = gGeoManager->FindNode(pos_hit.x(),pos_hit.y(),pos_hit.z());
	TString volname = node->GetVolume()->GetName();
	if(fDebug)std::cout<<"name: "<<volname<<"   pos: "<<pos_hit.x()<<" "<<pos_hit.y()<<" "<<pos_hit.z()<<std::endl;
	/*
	   size = fHitOutputArray->GetEntriesFast();
	   new ((*fHitOutputArray)[size]) EiccFdtHit( trackid, 
	   fCurrentEiccFdtPoint->GetDetectorID(),
	   iFdtPoint);
	 */
	if(fDebug)std::cout<<"Size: "<<size<<std::endl;

	//X[iFdtPoint] = pos_hit.x();  Y[iFdtPoint] = pos_hit.y();  Z[iFdtPoint] = pos_hit.z();
	//PX[iFdtPoint] = mom_hit.x();  PY[iFdtPoint] = mom_hit.y();  PZ[iFdtPoint] = mom_hit.z();
	//DetID[iFdtPoint] = fCurrentEiccFdtPoint->GetDetectorID(); 
	
	X[size] = pos_hit.x();  Y[size] = pos_hit.y();  Z[size] = pos_hit.z();
	PX[size] = mom_hit.x();  PY[size] = mom_hit.y();  PZ[size] = mom_hit.z();
	DetID[size] = fCurrentEiccFdtPoint->GetDetectorID(); 
        size++;

    }//end for EiccFdtiFdtPoint

    if(fDebug)std::cout<<"Get the hits information"<<std::endl;

    // How can I get the MC truth information ? talk to YT 





    // true start values
    //momentum  = sqrt(PX[0]*PX[0]+PY[0]*PY[0]+PZ[0]*PZ[0]);
    momentum  = TruP;
    TVector3 pos(0, 0, 0);
    //TVector3 mom(1.,0,0);
    //TVector2 mom(PX[0],PY[0],PZ[0]);
    TVector3 mom(TruPx,TruPy,TruPz);
    TVector3 mom_ref(TruPx,TruPy,TruPz);

    //mom.SetPhi(gRandom->Uniform(0.,2*TMath::Pi()));
    //mom.SetTheta(theta*TMath::Pi()/180);
    //mom.SetMag(momentum);

    TMatrixDSym covM(6);
    for (int i = 0; i < 3; ++i)
	covM(i,i) = resolution*resolution;
    for (int i = 3; i < 6; ++i)
	covM(i,i) = pow(resolution / nMeasurements / sqrt(3), 2);

    if (fDebug) {
	std::cout << "start values \n";
	pos.Print();
	mom.Print();
    }

    // calc helix parameters
    // genfit::HelixTrackModel* helix = new genfit::HelixTrackModel(pos, mom, charge);
    // // measurementCreator.setTrackModel(helix);

    // smeared start values
    TVector3 posM(pos);
    TVector3 momM(mom);
    if (smearPosMom) {
	posM.SetX(gRandom->Gaus(posM.X(),posSmear));
	posM.SetY(gRandom->Gaus(posM.Y(),posSmear));
	posM.SetZ(gRandom->Gaus(posM.Z(),zSmearFac*posSmear));

	momM.SetPhi(gRandom->Gaus(mom.Phi(),momSmear));
	momM.SetTheta(gRandom->Gaus(mom.Theta(),momSmear));
	momM.SetMag(gRandom->Gaus(mom.Mag(), momMagSmear*mom.Mag()));
    }


    // trackrep for creating measurements
    // double sign(1.);
    // if (chargeSwitchProb > gRandom->Uniform(1.))
    //     sign = -1.;
    // genfit::AbsTrackRep* rep = new genfit::RKTrackRep(sign*pdg);
    // sign = 1.;
    // if (chargeSwitchProb > gRandom->Uniform(1.))
    //     sign = -1.;
    // genfit::AbsTrackRep* secondRep = new genfit::RKTrackRep(sign*-211);

    if(fDebug) std::cout<<"The PID is "<<pdg<<std::endl;


    genfit::AbsTrackRep* rep = new genfit::RKTrackRep(pdg);
    genfit::AbsTrackRep* secondRep = new genfit::RKTrackRep(pdg);

    genfit::MeasuredStateOnPlane stateRef(rep);
    rep->setPosMomCov(stateRef, pos, mom, covM);

    // smeared start state
    genfit::MeasuredStateOnPlane stateSmeared(rep);
    rep->setPosMomCov(stateSmeared, posM, momM, covM);

    //rep->setPropDir(1);

    if (!matFX) genfit::MaterialEffects::getInstance()->setNoEffects();

    // remember original initial state
    const genfit::StateOnPlane stateRefOrig(stateRef);

    // create smeared measurements
    //std::vector< std::vector<genfit::AbsMeasurement*> > measurements;
    std::vector<genfit::PlanarMeasurement*>  measurements;

    std::vector<bool> outlierTrue;
    //bool outlier;
    // true values for left right. 0 for non wire measurements
    std::vector<int> leftRightTrue;
    //int lr;


    if(fDebug)std::cout<<"start to insert the measurement to genfit"<<std::endl;
    // create the measurement from input file
    //for (int i = 1; i < nEiccFdtPoints; ++i) // Why do I skip the first hit ??
    //for (int i = 0; i < nEiccFdtPoints; ++i) // Why do I skip the first hit ??

    bool add_origin = 1;

    double detectorResolution(0.001); // resolution of planar detectors
    TMatrixDSym hitCov(2);
    hitCov.UnitMatrix();
    hitCov *= detectorResolution*detectorResolution; // what's the role of hitCov ? -- Guoaq/2021/04/19
    
    // add some planar hits to track with coordinates I just made up
    TVectorD hitCoords(2);
    hitCoords[0] = 0;
    hitCoords[1] = 0;
    
    const int detId(0); // detector ID
    int planeId(0); // detector plane ID
    int hitId(0); // hit ID
    
    //   O, position of the virtual plane, U and V direction of the virtual plane
    TVector3 O(0,0,0), U(1,0,0), V(0,1,0);
    
    if(add_origin)
    {
	genfit::PlanarMeasurement* measurement = new genfit::PlanarMeasurement(hitCoords, hitCov, detId, ++hitId, nullptr);
	measurement->setPlane(genfit::SharedPlanePtr(new genfit::DetPlane(O, U, V)), ++planeId);
	measurements.push_back(measurement);
    }


    for (int i = 0; i < size; ++i) // Why do I skip the first hit ??
    {

	genfit::PlanarMeasurement* measurement = new genfit::PlanarMeasurement(hitCoords, hitCov, detId, ++hitId, nullptr);

	double sm_X(0), sm_Y(0), sm_Z(0);
	smearpixel(pixel_size, DetID[i], X[i],Y[i],Z[i],sm_X, sm_Y, sm_Z);


	if(smearOn)
	{
	    O.SetX(sm_X);
	    O.SetY(sm_Y);
	    O.SetZ(sm_Z);
	}
	else
	{
	    O.SetX(X[i]);
	    O.SetY(Y[i]);
	    O.SetZ(Z[i]);
	}

	// sould I rorate the plane of the detector ?? 
	U.SetX(1);
	U.SetY(0);
	U.SetZ(0);

	//std::cout<<"U before rotation "<<U.x()<<","<<U.y()<<","<<U.z()<<std::endl;
	//U.RotateY(-0.08); // rotate the x axis by 80 mrad for fdt
	//std::cout<<"U after rotation "<<U.x()<<","<<U.y()<<","<<U.z()<<std::endl;

	V.SetX(0);
	V.SetY(1);
	V.SetZ(0);

	//std::cout<<"O "<<O.X()<<", "<<O.Y()<<", "<<O.Z()<<"  U "<<U.X()<<", "<<U.Y()<<", "<<U.Z()<<" V "<<V.X()<<", "<<V.Y()<<", "<<V.Z()<<std::endl;
	measurement->setPlane(genfit::SharedPlanePtr(new genfit::DetPlane(O, U, V)), ++planeId);
	measurements.push_back(measurement);

    }


    //}
    //catch(genfit::Exception& e){
    //    std::cerr<<"Exception, next track"<<std::endl;
    //    std::cerr << e.what();
    //    continue; // here is a memleak!
    //}

if (fDebug) std::cout << "... done creating measurements \n";


if(fDebug)std::cout<<"initial array"<<std::endl;
for(int ik=0;ik<nMaxHit;ik++)
{

    X[ik]=-100;
    Y[ik]=-100;
    Z[ik]=-100;
    PX[ik]=-100;
    PY[ik]=-100;
    PZ[ik]=-100;
    DetID[ik]=-100;


}

// create track
TVectorD seedState(6);
TMatrixDSym seedCov(6);
rep->get6DStateCov(stateSmeared, seedState, seedCov);
fitTrack = new genfit::Track(rep, seedState, seedCov); //initialized with smeared rep
secondTrack = new genfit::Track(rep->clone(), seedState, seedCov); //initialized with smeared rep
if (twoReps) {
    fitTrack->addTrackRep(secondRep);
    secondTrack->addTrackRep(secondRep->clone());
}
else
delete secondRep;
//if (fDebug) fitTrack->Print("C");

fitTrack->checkConsistency();
//fitTrack->addTrackRep(rep->clone()); // check if everything works fine with more than one rep

if(fDebug)std::cout<<"size of measurement is "<<measurements.size()<<std::endl;
//if(measurements.size()>8 || measurements.size()<1) return;


if(measurements.size()<3)
{

    fouteff<<fEventNo<<","<<mom_ref.Px()<<","<<mom_ref.Py()<<","<<mom_ref.Pz()<<","<<mom_ref.Mag()<<","<<mom_ref.Pt()<<","<<mom_ref.Theta()<<","<<mom_ref.PseudoRapidity()<<","<<fs<<std::endl;
    return;

}


NHit = measurements.size();


// add measurements
for(unsigned int i=0; i<measurements.size(); ++i){
    /*
       if (splitTrack > 0 && (int)i >= splitTrack)
       break;
       if (i>0 && hitSwitchProb > gRandom->Uniform(1.))
       fitTrack->insertPoint(new genfit::TrackPoint(measurements[i], fitTrack), -2);
       else
     */
    fitTrack->insertPoint(new genfit::TrackPoint(measurements[i], fitTrack));

    fitTrack->checkConsistency();
    //if (fDebug) fitTrack->Print("C");
}
/*
   if (splitTrack > 0) {
   for(unsigned int i=splitTrack; i<measurements.size(); ++i){
   if (i>0 && hitSwitchProb > gRandom->Uniform(1.))
   secondTrack->insertPoint(new genfit::TrackPoint(measurements[i], secondTrack), -2);
   else
   secondTrack->insertPoint(new genfit::TrackPoint(measurements[i], secondTrack));

//if (fDebug) fitTrack->Print("C");
}
}
 */
fitTrack->checkConsistency();
secondTrack->checkConsistency();

//if (fDebug) fitTrack->Print();

//std::cout<<splitTrack<<" ================ "<<std::endl;

// do the fit
try{
    if (fDebug) std::cout<<"Starting the fitter"<<std::endl;

    if (prefit) {
	genfit::KalmanFitter prefitter(1, dPVal);
	prefitter.setMultipleMeasurementHandling(genfit::weightedClosestToPrediction);
	prefitter.processTrackWithRep(fitTrack, fitTrack->getCardinalRep());
    }

    fitter->processTrack(fitTrack, resort);
    if (splitTrack > 0)
	fitter->processTrack(secondTrack, resort);

    if (fDebug) std::cout<<"fitter is finished!"<<std::endl;
}
catch(genfit::Exception& e){
    std::cerr << e.what();
    std::cerr << "Exception, next track" << std::endl;
    fouteff<<fEventNo<<","<<mom_ref.Px()<<","<<mom_ref.Py()<<","<<mom_ref.Pz()<<","<<mom_ref.Mag()<<","<<mom_ref.Pt()<<","<<mom_ref.Theta()<<","<<mom_ref.PseudoRapidity()<<","<<fs<<std::endl;
    return;
}

if (splitTrack > 0) {
    if (fDebug) fitTrack->Print("C");
    if (fDebug) secondTrack->Print("C");

    if (fullMeasurement) {
	genfit::FullMeasurement* fullM = new genfit::FullMeasurement(secondTrack->getFittedState());
	fitTrack->insertPoint(new genfit::TrackPoint(fullM, fitTrack));
    }
    else
	fitTrack->mergeTrack(secondTrack);

    if (fDebug) fitTrack->Print("C");

    try{
	if (fDebug) std::cout<<"Starting the fitter"<<std::endl;
	fitter->processTrack(fitTrack, resort);
	if (fDebug) std::cout<<"fitter is finished!"<<std::endl;
    }
    catch(genfit::Exception& e){
	std::cerr << e.what();
	std::cerr << "Exception, next track" << std::endl;
	return;
    }
}


if (refit && !fitTrack->getFitStatus(rep)->isFitConverged()) {
    std::cout<<"Trying to fit again "<<std::endl;
    fitter->processTrack(fitTrack, resort);
}



if (fDebug) {
    fitTrack->Print("C");
    fitTrack->getFitStatus(rep)->Print();
}

fitTrack->checkConsistency();
secondTrack->checkConsistency();


#ifndef VALGRIND
if (!onlyDisplayFailed) {
    std::vector<genfit::Track*> event;
    event.push_back(fitTrack);
    if (splitTrack > 0)
	event.push_back(secondTrack);
    if(DisplayEvent)display->addEvent(event);
}
else if (onlyDisplayFailed &&
	(!fitTrack->getFitStatus(rep)->isFitConverged() ||
	 fitTrack->getFitStatus(rep)->getPVal() < 0.01)) {
    // add track to event display
    if(DisplayEvent)display->addEvent(fitTrack);
}
#endif


if (fitTrack->getFitStatus(rep)->isFitConverged()) {
    nTotalIterConverged += static_cast<genfit::KalmanFitStatus*>(fitTrack->getFitStatus(rep))->getNumIterations();
    nConvergedFits += 1;
}
else {
    nTotalIterNotConverged += static_cast<genfit::KalmanFitStatus*>(fitTrack->getFitStatus(rep))->getNumIterations();
    nUNConvergedFits += 1;
}

if (twoReps) {
    if (fitTrack->getFitStatus(secondRep)->isFitConverged()) {
	nTotalIterSecondConverged += static_cast<genfit::KalmanFitStatus*>(fitTrack->getFitStatus(secondRep))->getNumIterations();
	nConvergedFitsSecond += 1;
    }
    else {
	nTotalIterSecondNotConverged += static_cast<genfit::KalmanFitStatus*>(fitTrack->getFitStatus(secondRep))->getNumIterations();
	nUNConvergedFitsSecond += 1;
    }
}



// check if fit was successful
if (! fitTrack->getFitStatus(rep)->isFitConverged()) {
    if(fDebug)std::cout << "Track could not be fitted successfully! Fit is not converged! \n";
    //return;
}

if(fitTrack->getFitStatus(rep)->isFitConverged()) fs = 1;

genfit::TrackPoint* tp = fitTrack->getPointWithMeasurementAndFitterInfo(0, rep);
if (tp == nullptr) {
    std::cout << "Track has no TrackPoint with fitterInfo! \n";
    fouteff<<fEventNo<<","<<mom_ref.Px()<<","<<mom_ref.Py()<<","<<mom_ref.Pz()<<","<<mom_ref.Mag()<<","<<mom_ref.Pt()<<","<<mom_ref.Theta()<<","<<mom_ref.PseudoRapidity()<<","<<fs<<std::endl;
    return;
}
genfit::KalmanFittedStateOnPlane kfsop(*(static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(rep))->getBackwardUpdate()));
if (fDebug) {
    std::cout << "state before extrapolating back to reference plane \n";
    kfsop.Print();
}

// extrapolate back to reference plane.
try{
    //rep->extrapolateToPlane(kfsop, stateRefOrig.getPlane());;
    rep->extrapolateToPoint(kfsop, TVector3(0, 0, 0),false); // Changed by Guoaq to get the DCA; Should I do the extroplation ??? Probably it is unecessary !! 
}
catch(genfit::Exception& e){
    std::cerr<<"Exception, next track"<<std::endl;
    std::cerr << e.what();
    fouteff<<fEventNo<<","<<mom_ref.Px()<<","<<mom_ref.Py()<<","<<mom_ref.Pz()<<","<<mom_ref.Mag()<<","<<mom_ref.Pt()<<","<<mom_ref.Theta()<<","<<mom_ref.PseudoRapidity()<<","<<fs<<std::endl;
    return;
}

TVector3 Pos_sta = kfsop.getPos();
TVector3 Mom_sta = kfsop.getMom();
TVector3 Dir_sta = kfsop.getDir();

#ifndef VALGRIND
// calculate pulls
const TVectorD& referenceState = stateRefOrig.getState();

const TVectorD& state = kfsop.getState();
const TMatrixDSym& cov = kfsop.getCov();

double pval = fitter->getPVal(fitTrack, rep);
//assert( fabs(pval - static_cast<genfit::KalmanFitStatus*>(fitTrack->getFitStatus(rep))->getBackwardPVal()) < 1E-10 );
//std::cout<<"ini plane O, U, and V are : ["<<stateRefOrig.getPlane()->getO().X()<<", "<<stateRefOrig.getPlane()->getO().Y()<<", "<<stateRefOrig.getPlane()->getO().Z()<<"], ["<<stateRefOrig.getPlane()->getU().X()<<", "<<stateRefOrig.getPlane()->getU().Y()<<", "<<stateRefOrig.getPlane()->getU().Z()<<"], ["<<stateRefOrig.getPlane()->getV().X()<<", "<<stateRefOrig.getPlane()->getV().Y()<<", "<<stateRefOrig.getPlane()->getV().Z()<<"], "<<std::endl;
if(fDebug)std::cout<<"ini plane n : ["<<stateRefOrig.getPlane()->getNormal().X()<<", "<<stateRefOrig.getPlane()->getNormal().Y()<<", "<<stateRefOrig.getPlane()->getNormal().Z()<<"] "<<std::endl;
if(fDebug)std::cout<<"\n"<<std::endl;
if(fDebug)std::cout<<"fit P is "<<charge/state[0]<<", reference P is "<<momentum<<std::endl;
if(fDebug)std::cout<<"fit up is "<<state[1]<<", reference up is "<<referenceState[1]<<std::endl;
if(fDebug)std::cout<<"fit vp is "<<state[2]<<", reference vp is "<<referenceState[2]<<std::endl;
if(fDebug)std::cout<<"\n"<<std::endl;

// Get the momentum resolution and DCA
TVector3 GenFit_mom = kfsop.getMom();
double Achi2  = fitTrack->getFitStatus(rep)->getChi2();

if(fDebug)std::cout<<"true Pt is "<<mom.Pt()<<", measured Pt is "<<GenFit_mom.Pt()<<std::endl;

// calculate the DCAr
//
TVector3 DCA_local(kfsop.getState()[3],kfsop.getState()[4],0);
DCA_local.RotateUz(GenFit_mom.Unit());


if(fDebug)std::cout<<"u is "<<kfsop.getState()[3]<<" v is "<<kfsop.getState()[4]<<std::endl;

//delete state_at_beam_line;

//fill tree
// px_truth = mom.Px(); py_truth = mom.Py(); pz_truth = mom.Pz(); p_truth = mom.Mag(); pt_truth = mom.Pt(); theta_truth = mom.Theta(); eta_truth = mom.PseudoRapidity();
// px_rec = GenFit_mom.Px(); py_rec = GenFit_mom.Py();  pz_rec = GenFit_mom.Pz(); p_rec = GenFit_mom.Mag(); pt_rec = GenFit_mom.Pt(); theta_rec = GenFit_mom.Theta(); eta_rec = GenFit_mom.PseudoRapidity();
// mom_res = ((GenFit_mom.Mag() - mom.Mag())/mom.Mag());


//fill tree

// px_truth = mom.Px(); py_truth = mom.Py(); pz_truth = mom.Pz(); p_truth = mom.Mag(); pt_truth = mom.Pt(); theta_truth = mom.Theta(); eta_truth = mom.PseudoRapidity();
// px_rec = GenFit_mom.Px(); py_rec = GenFit_mom.Py();  pz_rec = GenFit_mom.Pz(); p_rec = GenFit_mom.Mag(); pt_rec = GenFit_mom.Pt(); theta_rec = GenFit_mom.Theta(); eta_rec = GenFit_mom.PseudoRapidity();

px_truth = mom_ref.Px(); py_truth = mom_ref.Py(); pz_truth = mom_ref.Pz(); p_truth = mom_ref.Mag(); pt_truth = mom_ref.Pt(); theta_truth = mom_ref.Theta(); phi_truth = mom_ref.Phi(); eta_truth = mom_ref.PseudoRapidity();
px_rec = GenFit_mom.Px(); py_rec = GenFit_mom.Py();  pz_rec = GenFit_mom.Pz(); p_rec = GenFit_mom.Mag(); pt_rec = GenFit_mom.Pt(); theta_rec = GenFit_mom.Theta(); phi_rec = GenFit_mom.Phi(); eta_rec = GenFit_mom.PseudoRapidity();
mom_res_old = ((GenFit_mom.Mag() - mom.Mag())/mom.Mag());
mom_res = ((fabs(state[0])-fabs(referenceState[0]))*mom.Mag());

// dca_rphi = DCA_local.X();
// dca_z = DCA_local.Z();
// New way to get the vertex information        

// if(Pos_sta.Y()>0) dca_rphi = sqrt(Pos_sta.X()*Pos_sta.X()+Pos_sta.Y()*Pos_sta.Y());
// else dca_rphi = -sqrt(Pos_sta.X()*Pos_sta.X()+Pos_sta.Y()*Pos_sta.Y());

dca_rphi = sqrt(Pos_sta.X()*Pos_sta.X()+Pos_sta.Y()*Pos_sta.Y());

abs_dca_rphi = sqrt(Pos_sta.X()*Pos_sta.X()+Pos_sta.Y()*Pos_sta.Y());
dca_z = Pos_sta.Z();
dca_x = Pos_sta.X();
dca_y = Pos_sta.Y();
// End of the new way to get the vertex information     
chi2 = Achi2;
if(fDebug) std::cout<<"calculated vetex position are "<<DCA_local.X()<<", "<<DCA_local.Y()<<", "<<DCA_local.Z()<<std::endl;

// Save the output to a csv file
//fout<<fEventNo<<","<<px_truth<<","<<py_truth<<","<<pz_truth<<","<<p_truth<<","<<pt_truth<<","<<theta_truth<<","<<eta_truth<<","<<px_rec<<","<<py_rec<<","<<pz_rec<<","<<p_rec<<","<<pt_rec<<","<<theta_rec<<","<<eta_rec<<","<<mom_res_old<<","<<mom_res<<","<<dca_rphi<<","<<dca_z<<","<<dca_x<<","<<dca_y<<","<<chi2<<","<<NHit<<","<<fs<<std::endl;


fout<<fEventNo<<","<<px_truth<<","<<py_truth<<","<<pz_truth<<","<<p_truth<<","<<pt_truth<<","<<theta_truth<<","<<phi_truth<<","<<eta_truth<<","<<px_rec<<","<<py_rec<<","<<pz_rec<<","<<p_rec<<","<<pt_rec<<","<<theta_rec<<","<<phi_rec<<","<<eta_rec<<","<<mom_res_old<<","<<mom_res<<","<<dca_rphi<<","<<dca_z<<","<<dca_x<<","<<dca_y<<","<<chi2<<","<<NHit<<","<<fs<<std::endl;

fouteff<<fEventNo<<","<<mom_ref.Px()<<","<<mom_ref.Py()<<","<<mom_ref.Pz()<<","<<mom_ref.Mag()<<","<<mom_ref.Pt()<<","<<mom_ref.Theta()<<","<<mom_ref.PseudoRapidity()<<","<<fs<<std::endl;



//dca_rphi = kfsop.getState()[3]; //cmDCA_local.X()
//dca_z = kfsop.getState()[4]; //cm
//std::cout<<"Isfitted is "<<Isfitted<<std::endl;
//t_out->Fill();

size = fHitOutputArray->GetEntriesFast();

// new ((*fHitOutputArray)[size]) EiccFdtHit( 0,
// 	0,
// 	0, mom_res );

if(fabs(mom_res)>0.05&&fDebug){
    std::cout<<"------mom reconstruction problem   "<<GenFit_mom.Mag()<<"    "<<mom.Mag()<<std::endl ;

    for (int i = 1; i < nEiccFdtPoints; ++i)
    {

	std::cout<<"xyz: "<<X[i]<<"  "<<Y[i]<<"  "<<Z[i]<<std::endl;

    }
}

#endif

measurements.clear();
if(fDebug)std::cout<<"========= end an event ==========================="<<std::endl;


if(fVerbose>0&&fDebug) {
    std::cout<<fHitOutputArray->GetEntriesFast() <<" Hits created out of "
	<<fPointArray->GetEntriesFast() <<" Points"<<std::endl;
}

}
// -------------------------------------------------------------------------
void EiccFdtIdealRecoTask::InitTransMat()
{
    //     std::cout<<"InitTransMat() with "<<fCurrentEiccFdtPoint->GetDetName()<<std::endl;
    /*
       gGeoManager->cd(
       fGeoH->GetPath( fCurrentEiccFdtPoint->GetSensorID() ).Data()
       );
     */
    fCurrentTransMat = gGeoManager->GetCurrentMatrix();
    if (fVerbose > 1) {
	fCurrentTransMat->Print("");
    }
}


void EiccFdtIdealRecoTask::smear(TVector3& pos)
{
    /// smear a 3d vector

    Double_t sigx=gRandom->Gaus(0,fSigmaX);
    Double_t sigy=gRandom->Gaus(0,fSigmaY);
    Double_t sigz=gRandom->Gaus(0,fSigmaZ);

    Double_t x = pos.x() + sigx;
    Double_t y = pos.y() + sigy;
    Double_t z = pos.z() + sigz;

    if (fVerbose > 1) {
	std::cout<<"EiccFdtIdealRecoTask::smear Point (x,y,z)=("
	    <<pos.x()<<","<<pos.z()<<","<<pos.z()<<") by ("
	    <<fSigmaX<<","<<fSigmaY<<","<<fSigmaZ<<") to ";
    }
    pos.SetXYZ(x,y,z);
    if (fVerbose > 1) {
	std::cout<<"("<<pos.x()<<","<<pos.y()<<","<<pos.z()<<")"<<std::endl;
    }
    return;
}


void EiccFdtIdealRecoTask::smearLocal(TVector3& pos)
{
    /// smear a 3d vector in the local sensor plane
    if (fVerbose > 1) {
	std::cout<<"EiccFdtIdealRecoTask::smearLocal"<<std::endl;
    }
    Double_t posLab[3], posSens[3];

    posLab[0]=pos.x();  posLab[1]=pos.y();  posLab[2]=pos.z();
    fCurrentTransMat->MasterToLocal(posLab,posSens);

    pos.SetXYZ(posSens[0],posSens[1],posSens[2]);

    smear(pos); // apply a gaussian

    posSens[0]=pos.x();  posSens[1]=pos.y();  posSens[2]=pos.z();
    fCurrentTransMat->LocalToMaster(posSens,posLab);
    pos.SetXYZ(posLab[0],posLab[1],posLab[2]);


    //   TMatrixT<Double_t> cov(3,3);
    //   cov[0][0]=fSigmaX; cov[0][1]=0.;      cov[0][2]=0.;
    //   cov[1][0]=0.;      cov[1][1]=fSigmaY; cov[1][2]=0.;
    //   cov[2][0]=0.;      cov[2][1]=0.;      cov[2][2]=fSigmaZ;

    //   TMatrixT<Double_t> rot(3,3);
    //   rot[0][0] =  fCurrentTransMat->GetRotationMatrix()[0];
    //   rot[0][1] =  fCurrentTransMat->GetRotationMatrix()[1];
    //   rot[0][2] =  fCurrentTransMat->GetRotationMatrix()[2];
    //   rot[1][0] =  fCurrentTransMat->GetRotationMatrix()[3];
    //   rot[1][1] =  fCurrentTransMat->GetRotationMatrix()[4];
    //   rot[1][2] =  fCurrentTransMat->GetRotationMatrix()[5];
    //   rot[2][0] =  fCurrentTransMat->GetRotationMatrix()[6];
    //   rot[2][1] =  fCurrentTransMat->GetRotationMatrix()[7];
    //   rot[2][2] =  fCurrentTransMat->GetRotationMatrix()[8];

    //   fHitCovMatrix = cov;
    //   fHitCovMatrix *= rot;

    return;
}



void EiccFdtIdealRecoTask::CalcDetPlane(TVector3& oVect, TVector3& uVect,TVector3& vVect)
{
    Double_t O[3], U[3], V[3], o[3], u[3], v[3];
    O[0]=oVect.x();  O[1]=oVect.y();  O[2]=oVect.z();
    U[0]=uVect.x();  U[1]=uVect.y();  U[2]=uVect.z();
    V[0]=vVect.x();  V[1]=vVect.y();  V[2]=vVect.z();

    /*
       if (fVerbose > 1) {
       std::cout<<"EiccFdtIdealRecoTask::CalcDetPlane from Detector "
       <<fCurrentEiccFdtPoint->GetSensorID()<<std::endl;
       }
     */

    //make transformation
    fCurrentTransMat->LocalToMaster(O,o);
    fCurrentTransMat->LocalToMaster(U,u);
    fCurrentTransMat->LocalToMaster(V,v);
    oVect.SetXYZ(o[0],o[1],o[2]);
    uVect.SetXYZ(u[0],u[1],u[2]);
    vVect.SetXYZ(v[0],v[1],v[2]);
}



int EiccFdtIdealRecoTask::randomPdg() {
    int pdg_rand;

    switch(int(gRandom->Uniform(8))) {
	case 1:
	    pdg_rand = -11; break;
	case 2:
	    pdg_rand = 11; break;
	case 3:
	    pdg_rand = 13; break;
	case 4:
	    pdg_rand = -13; break;
	case 5:
	    pdg_rand = 211; break;
	case 6:
	    pdg_rand = -211; break;
	case 7:
	    pdg_rand = 2212; break;
	default:
	    pdg_rand = 211;
    }

    return pdg_rand;
}


int EiccFdtIdealRecoTask::randomSign() {
    if (gRandom->Uniform(1) > 0.5)
	return 1;
    return -1;
}

void EiccFdtIdealRecoTask::smearpixel(double pixelsize, int det_id, double input_X, double input_Y, double input_Z, double &output_X, double &output_Y, double &output_Z)
{

    //for the edt; 

    TVector3 S(gRandom->Gaus(0,pixelsize/sqrt(12)),gRandom->Gaus(0,pixelsize/sqrt(12)),0);
    // S.RotateY(-0.08); // rotate the x axis by 80 mrad for Fdt

    output_X = S.x() + input_X;
    output_Y = S.y() + input_Y;
    output_Z = S.z() + input_Z;
}


ClassImp(EiccFdtIdealRecoTask);
